/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

#include "joystickPage.h"
#include "vcConfig.h"
#include "joystickThread.h"

/*<---Extern Variables---->*/
extern GuiStruct guiStr;
extern FifoMsg_t  * fifo;
extern Client * js_client;
extern bool joystickDevEnabled;

Servos_t servo_values;
Buttons_t button_values;

float devices[MAX_CLIENTS];

bool JoystickPage::create_page(Client *client, GtkBuilder *builder)
{	
	if( !client ) {
		cout << "JoystickPage::create_page -- invalid client " << endl;
		return false;
	}
	if( page ) {
		cout << "JoystickPage::create_page -- page has already been created" << endl;
		return true;
	}
	if( !builder ) {
		cout << "JoystickPage::create_page -- invalid xml builder" << endl;
		return false;
	}

	servo_values.channels[0] = 0;
	servo_values.channels[1] = 0;
	servo_values.channels[2] = 0;
	servo_values.channels[3] = 0;

	button_values.channels[0] = 0;
	button_values.channels[1] = 0;
	button_values.channels[2] = 0;
	button_values.channels[3] = 0;

	page_client        = client;
	page               = GTK_WIDGET (gtk_builder_get_object(builder, "joystick_page"));
	axis0_slider    = GTK_WIDGET (gtk_builder_get_object(builder, "axis0_slider"));
	axis1_slider    = GTK_WIDGET (gtk_builder_get_object(builder, "axis1_slider"));
	axis2_slider    = GTK_WIDGET (gtk_builder_get_object(builder, "axis2_slider"));
	axis3_slider    = GTK_WIDGET (gtk_builder_get_object(builder, "axis3_slider"));
	button0    = GTK_WIDGET (gtk_builder_get_object(builder, "button0"));
	button1    = GTK_WIDGET (gtk_builder_get_object(builder, "button1"));
	button2    = GTK_WIDGET (gtk_builder_get_object(builder, "button2"));
	button3    = GTK_WIDGET (gtk_builder_get_object(builder, "button3"));
	joystick_enable = GTK_WIDGET (gtk_builder_get_object(builder, "joystick_enable"));
	joystick_stop  = GTK_WIDGET (gtk_builder_get_object(builder, "joystick_stop"));

	// make sure we got all of our widgets
	if( !( page && axis0_slider && axis1_slider && axis2_slider && axis3_slider && button0 && button1 && button2 && button3 && joystick_enable && joystick_stop) )
	{
		cout << "JoystickPage::create_page - did not find all required widgets in xml file" << endl;	
		page = NULL;
		return false;
	}

	g_signal_connect(G_OBJECT(joystick_enable), "clicked", G_CALLBACK(JoystickPage::joystick_enable_clicked), this);
	g_signal_connect(G_OBJECT(joystick_stop), "clicked", G_CALLBACK(JoystickPage::joystick_stop_clicked), this);

	return true;
}

void JoystickPage::update()
{
	//-- See if the page has been initialized
	if( !page_client ) {
		cout << "JoystickPage::update - invalid page_client" << endl;
		return;
	}
	if( page == NULL ) {
		cout << "JoystickPage::update -- page has not been created" << endl;
		return;
	}

	// don't do work if we don't need too
	if( !GTK_WIDGET_VISIBLE( page ) ) {
		return;
	}

	set_axis_and_button_states();

	if(joystickDevEnabled)
	{
		if(js_client != NULL)
			if((js_client->name) == (page_client->name))
				set_good_state(GTK_WIDGET(joystick_enable));
			else
				set_rx_state(GTK_WIDGET(joystick_enable),true);
		else
			set_rx_state(GTK_WIDGET(joystick_enable),true);
	} else {
		if(js_client != NULL) {
			set_bad_state(GTK_WIDGET(joystick_enable));
		}
	}

	// redraw page
	gtk_widget_queue_draw(page);	
}

void JoystickPage::joystick_enable_clicked(GtkButton *button, gpointer data)
{
	//cout << "JoystickPage::track_nodes_clicked" << endl;
	JoystickPage *joystick = (JoystickPage *)data;
	if( !joystick )
		return;

	joystick->set_js_client();

	joystick->set_sent_state(GTK_WIDGET(button));

	return;
}

void JoystickPage::joystick_stop_clicked(GtkButton *button, gpointer data)
{
	//cout << "JoystickPage::track_nodes_clicked" << endl;
	JoystickPage *joystick = (JoystickPage *)data;
	if( !joystick )
		return;

	joystick->unset_js_client();

	return;
}

void JoystickPage::set_js_client()
{	
	js_client = guiStr.shm_ptr->clients.getClient(page_client->name);
}

void JoystickPage::unset_js_client()
{	
	set_rx_state(joystick_enable, 1);
	js_client = NULL;
}

void JoystickPage::set_axis_and_button_states() {
	gtk_range_set_value(GTK_RANGE(axis0_slider),(float)(servo_values.channels[0])/327.625-100.0);
	gtk_range_set_value(GTK_RANGE(axis1_slider),(float)(servo_values.channels[1])/327.625-100.0);
	gtk_range_set_value(GTK_RANGE(axis2_slider),(float)(servo_values.channels[2])/327.625-100.0);
	gtk_range_set_value(GTK_RANGE(axis3_slider),(float)(servo_values.channels[3])/327.625-100.0);

	gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(button0),button_values.channels[0]);
	gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(button1),button_values.channels[1]);
	gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(button2),button_values.channels[2]);
	gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(button3),button_values.channels[3]);

	/*set_rx_state(button0, button_values.channels[0]);
	set_rx_state(button1, button_values.channels[1]);
	set_rx_state(button2, button_values.channels[2]);
	set_rx_state(button3, button_values.channels[3]);*/

}

void JoystickPage::setCMD_RX(uint8_t cmd_id, int ack_nack)
{
	uint8_t * dataPtr = CMD_DATAPTR(fifo->data);
	switch( cmd_id ) {
	//FIXME -- instead of getting these values from CMD_SERVO below they should probably be updated from shared memory
		case CMD_SERVO:
      memcpy(&servo_values,dataPtr,sizeof(Servos_t));
			//printf("(%d,%d,%d,%d)\n",servo_values.channels[0],servo_values.channels[1],servo_values.channels[2],servo_values.channels[3]);
			break;

		case CMD_BUTTONS:
      memcpy(&button_values,dataPtr,sizeof(Buttons_t));
			break;

		default:
			break;
	}

	// update page
	if( page )
		gtk_widget_queue_draw(page);
}
